package edu.buaa.act.model.tsc;

import com.graphhopper.matching.EdgeMatch;
import com.graphhopper.matching.GPXExtension;
import com.graphhopper.matching.MapMatching;
import com.graphhopper.matching.MatchResult;
import com.graphhopper.routing.AlgorithmOptions;
import com.graphhopper.routing.util.CarFlagEncoder;
import com.graphhopper.routing.util.EncodingManager;
import com.graphhopper.routing.weighting.FastestWeighting;
import com.graphhopper.routing.weighting.Weighting;
import com.graphhopper.util.DistanceCalc;
import com.graphhopper.util.DistancePlaneProjection;
import com.graphhopper.util.GPXEntry;
import com.graphhopper.util.Parameters;
import com.graphhopper.util.PointList;
import edu.buaa.act.helper.MyGraphHopperOSM;
import edu.buaa.act.helper.Utils;
import edu.buaa.act.model.Trajectory;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;

import java.io.File;
import java.io.FileNotFoundException;
import java.io.IOException;
import java.util.ArrayList;
import java.util.Calendar;
import java.util.List;
import java.util.TimeZone;

/**
 * Created by T7810 on 2017/3/12.
 * ref: https://github.com/graphhopper/graphhopper/blob/master/docs/core/low-level-api.md
 */
public class MyMapMatching
{
    private static Logger log = Utils.getDebugLogger();

    public static DistanceCalc distanceCalc = new DistancePlaneProjection();
    private MyGraphHopperOSM hopper = new MyGraphHopperOSM();
    public MapMatching mapMatching;

    public MyMapMatching(File PbfMapFile, File tmpDir, double gpsAccuracy) throws IOException
    {
//        hopper.forServer();
        CarFlagEncoder encoder = new CarFlagEncoder();
        // import OpenStreetMap data
        hopper.setDataReaderFile(PbfMapFile.getAbsolutePath());
        hopper.setGraphHopperLocation(tmpDir.getAbsolutePath());
        hopper.setEncodingManager(new EncodingManager(encoder));
        hopper.getCHFactoryDecorator().setEnabled(false);
        hopper.importOrLoad();
        // create MapMatching object, can and should be shared across threads
        String algorithm = Parameters.Algorithms.DIJKSTRA_BI;
        Weighting weighting = new FastestWeighting(encoder);
        AlgorithmOptions algoOptions = AlgorithmOptions.start()
                .maxVisitedNodes(21_0000_0000)
                .algorithm(algorithm)
                .weighting(weighting)
                .build();

        mapMatching = new MapMatching(hopper, algoOptions);
        mapMatching.setMeasurementErrorSigma(gpsAccuracy);
    }

    public MatchResult matching(Trajectory traj)
    {
        MatchResult mr = mapMatching.doWork(traj.getGpsEntries());

        List<EdgeMatch> matches = mr.getEdgeMatches();
        if(matches.isEmpty()){
            throw new NoEdgeMatchedException();
        }
        List<TrajectoryRoadEntry> edges = calcTravelTime(traj, matches);

        traj.setRoads(edges);
        return mr;
    }

    /**
     * calculate travel time of each road along the trajectory.
     * we need this function because the HMM map-matching algorithm (used by graphHopper)
     * do not generate the travel time of each road.
     *
     * First, we transform each known gps entry point in the trajectory from [latitude, longitude, timestamp] to
     * [distance, timestamp], where the 'distance' is the distance (not Displacement) from current gps point
     * to the trajectory start gps point. We also transform query points (joint point of road) to distance.
     * Then we use linear interpolation to calculate the timestamp of query point.
     * Finally, the difference between the two timestamp of the joint point of road is the travel time.
     *
     *
     * @param traj
     * @param matches map-matching result
     * @return [roadID, timeSlot, travelTime] list
     */
    private List<TrajectoryRoadEntry> calcTravelTime(Trajectory traj, List<EdgeMatch> matches)
    {
        List<DistanceTimeEntry> knownPoints = new ArrayList<>();
        List<DistanceTimeEntry> queryPoints = new ArrayList<>();

        // collection all available [distance v.s. timestamp] data.
        double trajLen = calcDistanceTimeList(matches, knownPoints, queryPoints);

        traj.setLength(trajLen);

        if(knownPoints.size()<2) throw new CannotCalcTravelTimeException();

        // calc joint point timestamp from its distance(to trajectory start point) using linear interpolation.
        try {
            return calcTravelTime(knownPoints, queryPoints);
        }catch (CannotCalcTravelTimeException e){
//            long id = System.currentTimeMillis();
//            traj.toFile(new File(id+".gpx"));
            throw e;
        }
    }

    // @return traj length (in meter)
    private double calcDistanceTimeList(List<EdgeMatch> matches, List<DistanceTimeEntry> knownPoints, List<DistanceTimeEntry> queryPoints)
    {
        double disToTrajectoryStart = 0;
        for (EdgeMatch edge : matches) {
            List<GPXExtension> gpsPointOnTheWay = edge.getGpxExtensions();
            int k = 0;
            double disToWayStart = 0;
            PointList nodes = edge.getEdgeState().fetchWayGeometry(3);
            for (int j = 0; j < nodes.size() - 1; j++) { // loop through edges. edgeNumber = nodeNumber - 1
                double startLat = nodes.getLatitude(j);
                double startLon = nodes.getLongitude(j);
                double endLat = nodes.getLatitude(j + 1);
                double endLon = nodes.getLongitude(j + 1);
                ProjectionResult r;
                while (k < gpsPointOnTheWay.size()) {
                    GPXEntry gpxPoint = gpsPointOnTheWay.get(k).getEntry();
                    r = projectionPoint(
                            startLat, startLon,
                            endLat, endLon,
                            gpxPoint.getLat(), gpxPoint.getLon());
                    if (r.inside) {
                        double deltaDis = distanceCalc.calcDist(r.y, r.x, startLat, startLon);
                        double totalDis = deltaDis + disToWayStart + disToTrajectoryStart;
                        knownPoints.add(new DistanceTimeEntry(totalDis, gpxPoint.getTime()));
                        k++;
//                        System.out.printf("%f,%f\n", r.y, r.x);
//                        System.out.printf("%f,%d,know\n", totalDis, gpxPoint.getTime());
                    } else {
                        break;
                    }
                }
                double edgeLength = distanceCalc.calcDist(startLat, startLon, endLat, endLon);
                disToWayStart += edgeLength;
//                // query all nodes (include pillar node)
//                queryPoints.add(disToTrajectoryStart+disToWayStart);
//                queryIndex.add(knownPoints.size());
            }
            disToTrajectoryStart += disToWayStart;
            // only query tower nodes.
            queryPoints.add(new DistanceTimeEntry(disToTrajectoryStart, edge.getEdgeState().getEdge()));
//            queryIndex.add(knownPoints.size());
        }
        return disToTrajectoryStart;
    }

    // calc joint point timestamp from its distance(to trajectory start point) using linear interpolation.
    private List<TrajectoryRoadEntry> calcTravelTime(List<DistanceTimeEntry> known, List<DistanceTimeEntry> query)
    {
        List<TrajectoryRoadEntry> results = new ArrayList<TrajectoryRoadEntry>();
        int k=0;
        for(int i=0; i<query.size(); i++)
        {
            DistanceTimeEntry cur = query.get(i);
            while(k < known.size() && known.get(k).distance < cur.distance){ k++; }

            if(0<k && k<known.size()) // normal
            {
                DistanceTimeEntry pre = known.get(k-1);
                DistanceTimeEntry post = known.get(k);
                cur.time = (long) ( pre.time +
                        (post.time - pre.time) *
                                (cur.distance - pre.distance)/
                                (post.distance - pre.distance));
                if(i>0)
                {
                    DistanceTimeEntry last = query.get(i-1);
                    if(last.time!=0) {
                        long travelTime = (cur.time - last.time) / 1000;
                        if (travelTime < 0 ) {
                            log.error("invalid travel time({})! cur: {} pre: {}", travelTime, cur.time, last.time);
                        }else if( travelTime > 1800 ){
                            throw new CannotCalcTravelTimeException();
                        }else {
                            results.add(new TrajectoryRoadEntry(
                                    cur.edgeID,
                                    timestamp2TimeSlot(cur.time),
                                    (int) travelTime));
                        }
                    }
                }
            }else if(k==known.size()){
                // query a point after(k==know.size()) all know points. exit loop.
                break;
            }else{
                // query a point before(k==0) all know points. do nothing
            }
//            int index = queryIndex.get(i);
//            if(index>=known.size()){
//                break;
//            }else if(index-1>=0) {
//                DistanceTimeEntry pre = known.get(index - 1);
//                DistanceTimeEntry post = known.get(index);
//                cur.time = (long) ((cur.distance - pre.distance) * (post.time - pre.time) / (post.distance - pre.distance) + pre.time);
////                System.out.printf("%f,%d,query\n", dis, time);
//                if(i>0){
//                    results.add(new TrajectoryRoadEntry(
//                            cur.edgeID,
//                            timestamp2TimeSlot(cur.time),
//                            (int) ((cur.time - query.get(i - 1).time) / 1000)));
//                }
//            }else{
//                //continue;
//            }
        }
        return results;
    }

    private int timestamp2TimeSlot(long timestamp)
    {
        Calendar c = Calendar.getInstance(TimeZone.getTimeZone("GMT+8:00"));
        c.setTimeInMillis(timestamp);
        int timeSlot = c.get(Calendar.HOUR_OF_DAY)*2;
        if(c.get(Calendar.MINUTE)>=30){
            timeSlot++;
        }
        return timeSlot;
    }

    static ProjectionResult projectionPoint(double startY, double startX, double endY, double endX, double y, double x)
    {
//            System.out.printf("(%f %f) (%f %f) (%f %f) \n", startY, startX, endY, endX, y, x);
        double roadX = endX - startX;
        double roadY = endY - startY;
        double len = Math.hypot(roadX, roadY);
        double iX = roadX / len;
        double iY = roadY / len;
        double vX = x-startX;
        double vY = y-startY;
        double innerProduct = vX * roadX + vY * roadY;
        double projectionLen = innerProduct / len;
        double tX = startX + projectionLen * iX;
        double tY = startY + projectionLen * iY;
        return new ProjectionResult(tX, tY, projectionLen<=len);
    }

    private static class DistanceTimeEntry
    {
        public double distance;
        public long time;
        public int edgeID=-1;
        public DistanceTimeEntry(double dis, long t)
        {
            this.distance = dis;
            this.time = t;
        }
        public DistanceTimeEntry(double dis, int edgeID)
        {
            this.distance = dis;
            this.edgeID = edgeID;
        }
    }

    static class ProjectionResult
    {
        public double x;
        public double y;
        public boolean inside;
        public ProjectionResult(double x, double y, boolean b)
        {
            this.x = x;
            this.y = y;
            this.inside = b;
        }
    }

    public static class TrajectoryRoadEntry
    {
        public final int edgeID;
        public final int timeSlot;
        public final int travelTime; // in seconds;

        public TrajectoryRoadEntry(int edgeID, int timeSlot, int travelTime)
        {
            this.edgeID = edgeID;
            this.timeSlot = timeSlot;
            this.travelTime = travelTime;
        }

        public String toString(){
            return String.format("%d %d %d", edgeID, timeSlot, travelTime);
        }
    }


    private static void delete(File f) throws IOException
    {
        if (f.isDirectory()) {
            for (File c : f.listFiles())
                delete(c);
        }
        if (!f.delete())
            throw new FileNotFoundException("Failed to delete file: " + f);
    }



    public class NoEdgeMatchedException extends RuntimeException
    {

    }

    public class CannotCalcTravelTimeException extends RuntimeException
    {
    }
}


//            // direct access of matched edges.
//            // return GraphHopper edges with all associated GPX entries
//            List<EdgeMatch> matches = mr.getEdgeMatches();
//            // now do something with the edges like storing the edgeIds or doing fetchWayGeometry etc
//            int count=0;
//            for(EdgeMatch edge : matches)
//            {
//                System.out.println(edge);
////                System.out.print(edge.getEdgeState().getEdge());
//                for(GPXExtension point: edge.getGpxExtensions())
//                {
//                    GPXEntry gpxPoint = point.getEntry();
//                    QueryResult qResult = point.getQueryResult();
////                    qResult.ge
//                    System.out.println("|----"+qResult.toString());
//                }
//                count++;
////                if(count>10) break;
//            }



//            // fill GHResponse for identical structure
//            Path path = mapMatching.calcPath(mr);
//            Translation tr = trMap.getWithFallBack(locale);
//            new PathMerger().doWork(matchGHRsp, Collections.singletonList(path), tr);


//            System.out.printf("%6d %5d", edgeID, (int)roadLength);
////            System.out.printf("%6d %8d %5d", edgeID, (int)osmID, (int)roadLength);
//            if(indexMax!=-1){
//                long travelTime = gpsPointOnThatRoad.get(indexMax).getEntry().getTime() -
//                        gpsPointOnThatRoad.get(indexMin).getEntry().getTime();
//                System.out.printf("%5d",travelTime/1000); //Estimated travel time
//                System.out.printf("%3d", size); //points on road
//                System.out.printf("%6d/%d/%d\n", indexMin+1, indexMax+1, size);//nearest to startNode/is nearest to endNode.
//                totalPathTravelTime += travelTime;
//            }else{
//                System.out.println();
//            }
